#include "pid.h"


/*************************************************************************************************
 *	函 数 名:	PID_Calculate
 *
 *	函数功能:	PID计算公式
 *
 *   参    数：  PID结构体，当前值，目标值
 *************************************************************************************************/
float PID_Calculate(pid_t *pid, float Measure, float Target)
{
	pid->Target = Target;			  // 赋值目标值
	pid->Measure = Measure;			  // 赋值测量值
	pid->Error[0] = Target - Measure; // 计算当前误差

	pid->KpOut = pid->Kp * (pid->Error[0]);					// 比例计算，Kp*当前误差
	pid->KiOut += pid->Ki * pid->Error[0];					// 积分计算，Ki*误差累加值
	PID_Limit(&(pid->KiOut), pid->Ki_Limit_MAX);			// 积分限幅
	pid->KdOut = pid->Kd * (pid->Error[0] - pid->Error[1]); // 微分计算，Kd*(当前误差-上一次误差)

	pid->PID_Out = pid->KpOut + pid->KiOut + pid->KdOut; // PID计算
	PID_Limit(&(pid->PID_Out), pid->PID_Limit_MAX);		 // PID输出限幅

	pid->Error[2] = pid->Error[1];
	pid->Error[1] = pid->Error[0];

	return pid->PID_Out;
}

void Set_PID_Param(pid_t *pid, float P, float I, float D)
{
	pid->Kp = P;
	pid->Ki = I;
	pid->Kd = D;
	pid->KpOut = 0;
	pid->KiOut = 0;
	pid->KdOut = 0;
	pid->PID_Out = 0; // 输出清零
}

// PID限幅函数 *a传入要限幅的参数  ABS_MAX限幅大小
void PID_Limit(float *a, float ABS_MAX)
{
	if (*a > ABS_MAX)
		*a = ABS_MAX;
	if (*a < -ABS_MAX)
		*a = -ABS_MAX;
}
